﻿import cv2
import numpy as np
import time
import math
import global_var as glv
import visual_function as vf
from basic_game import basic_game

class mine(basic_game):
    def __init__(self, running = False):
        basic_game.__init__(self, running)
        ###云台初始化###
        self.PWM_Y = 2200 #云台角度初始化
        self.state = "change_PWM"
        basic_game.execute(self,self.PWM_Y)
        self.state = "change_PWM_X"
        basic_game.execute(self,1500)
        ###标志位###
        self.throughGate = False #已通过该关卡
        self.headUp = False #抬头调整位姿-大
        self.get_color = False #是否获得目标颜色区块
        self.get_line = False #是否能获得黑色车道线
        self.special = False
        ###阈值###
        self.target_color = "black"
        self.PWM_MaxY = 2200
        self.PWM_MinY = 1700
        self.minCenter = self.resize_width/2 - self.resize_width/12 #中心区域左边界
        self.maxCenter = self.resize_width/2 + self.resize_width/12 #中心区域右边界
        self.threahold_change_PWM = self.resize_height - self.resize_height/3 #在云台未到目标值时目标接近需要更高云台的阈值
        self.threahold_mineNear = self.resize_height - self.resize_height/3
        ###变量####
        self.lines = None
        self.circle = None

    def logic_position(self):
        if self.get_line:
            self.get_line = False
            if self.lines["left"] is not None and self.lines["right"] is not None:
                line_center = (self.lines["left"]["b"] - self.lines["right"]["b"])/(self.lines["right"]["k"] - self.lines["left"]['k'])
                if line_center > self.minCenter and line_center < self.maxCenter:
                    self.state = "change_PWM"
                    basic_game.execute(self, self.PWM_MaxY)
                    self.headUp = False
                elif line_center < self.minCenter:
                    self.state = "turn_left"
                    basic_game.execute(self)
                elif line_center > self.maxCenter:
                    self.state = "turn_right"
                    basic_game.execute(self)
            elif self.lines["left"] is not None:
                self.state = "turn_right"
                basic_game.execute(self)
            elif self.lines["right"] is not None:
                self.state = "turn_left"
                basic_game.execute(self)
            else:
                self.state = "change_PWM"
                basic_game.execute(self, self.PWM_MaxY)
                self.headUp = False
        else:
            self.state = "change_PWM"
            basic_game.execute(self, self.PWM_MaxY)
            self.headUp = False
    def logic_mine(self):
        if self.get_color :
            if self.circle["center_x"] > self.minCenter and self.circle["center_x"] < self.maxCenter:
                #判断自身位置决定执行左右绕过地雷的操作
                self.special = True
            elif self.circle["center_x"] <= self.minCenter:
                self.state = "turn_right"
                basic_game.execute(self)
            elif self.circle["center_x"] > self.maxCenter:  
                self.state = "turn_left"
                basic_game.execute(self)

        else:
            self.state = "go_straight"
            basic_game.execute(self)
            self.state = "change_PWM"
            basic_game.execute(self, self.PWM_MinY)
            self.headUp = True
    def logic_special(self):
        if self.get_line:
            self.get_line = False
            if self.lines["left"] is not None:
                self.state = "right_bypass"
                basic_game.execute(self)
            elif self.lines["right"] is not None:
                self.state = "left_bypass"
                basic_game.execute(self)
            else:
                self.state = "right_bypass"
                basic_game.execute(self)
        else:
            self.state = "right_bypass"
            basic_game.execute(self)
        self.special = False
    def run(self, hsvImage, outImage = None):
        if self.headUp and self.special is False:
            self.get_line, self.lines = vf.getLineByColor(hsvImage, 'black',outImage = outImage)
            print("line")
            print(self.state)
            if outImage is not None:
                cv2.imshow("mineLineframe",outImage)
                cv2.waitKey(1)
            mine.logic_position(self)
            time.sleep(2)
        elif self.headUp is False and self.special:
            self.get_line, self.lines = vf.getLineByColor(hsvImage, 'black',outImage = outImage)
            if outImage is not None:
                cv2.imshow("mineLineframe",outImage)
                cv2.waitKey(1)
            mine.logic_special(self)
            time.sleep(2)
        else:
            self.get_color, self.circle = vf.getAreaByColor(hsvImage, self.target_color, outImage, 2)
            print("detection_mine")
            print(self.state)
            if outImage is not None:
                cv2.imshow("mineframe",outImage)
                cv2.waitKey(1)
            ######
            if self.get_color is False:
                get_baffle, _,_ = vf.getAreaByColor(hsvImage, "blue", outImage, minArea = 2000)
                if get_baffle:
                    return True
            ######
            mine.logic_mine(self)
            time.sleep(2)
        return False
